#include "nscd_servo.h"

int ServoPosUs[11];
int ServoNum = 0;
int last_time = 0;

//set up GPIO and timer to control to all 10 servos
void Servo_Init(void)
{
	ServoNum = 0;
	last_time = 20000;
	int i;
	// reset servo position to center if necessary
	for(i=0; i<10; i++)
	{
		ServoPosUs[i] = 1500+i*100;//Pos[i];
	}

	// init GPIO
	GPIO_InitTypeDef GPIO_InitStructure;

	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);

	//GPIO_DeInit(GPIO_InitStructure);

	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_1 | GPIO_Pin_3| GPIO_Pin_4| GPIO_Pin_5| GPIO_Pin_8;

	GPIO_Init(GPIOB, &GPIO_InitStructure);


	// init interrupt
	NVIC_InitTypeDef NVIC_InitStructure;

	//enable tim2 irq
	NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;

	NVIC_Init(&NVIC_InitStructure);

	// init timers
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);

	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	//setting timer 2 interrupt to 1Mhz = ((36000000/36))
	TIM_TimeBaseStructure.TIM_Prescaler = 35 ;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Down;
	TIM_TimeBaseStructure.TIM_Period = 1000-1;
	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
	TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
	 /* TIM IT enable */
	TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);

	/* TIM2 enable counter */
	TIM_Cmd(TIM2, ENABLE);

}

//timer 2 interrupt
void TIM2_IRQHandler(void)
{

	//if interrupt happens the do this
	if(TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET)
	{
		//clear interrupt and start counting again to get precise freq
		TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
		//toggle led to see some chance in port A pin 9
		//GPIOB->ODR ^= GPIO_Pin_1;
		switch(ServoNum)
		{
		case 0:
			GPIOB->ODR |= GPIO_Pin_1;
			last_time -= ServoPosUs[ServoNum];
			TIM_SetCounter(TIM2,ServoPosUs[ServoNum++]);
			break;
		case 1:
			GPIOB->ODR &= ~GPIO_Pin_1;
			GPIOB->ODR |= GPIO_Pin_3;
			last_time -= ServoPosUs[ServoNum];
			TIM_SetCounter(TIM2,ServoPosUs[ServoNum++]);
			break;
		case 2:
			GPIOB->ODR &= ~GPIO_Pin_3;
			GPIOB->ODR |= GPIO_Pin_4;
			last_time -= ServoPosUs[ServoNum];
			TIM_SetCounter(TIM2,ServoPosUs[ServoNum++]);
			break;
		case 3:
			GPIOB->ODR &= ~GPIO_Pin_4;
			GPIOB->ODR |= GPIO_Pin_5;
			last_time -= ServoPosUs[ServoNum];
			TIM_SetCounter(TIM2,ServoPosUs[ServoNum++]);
			break;
		case 4:
			GPIOB->ODR &= ~GPIO_Pin_5;
			GPIOB->ODR |= GPIO_Pin_8;
			last_time -= ServoPosUs[ServoNum];
			TIM_SetCounter(TIM2,ServoPosUs[ServoNum++]);
			break;
		case 5:
			GPIOB->ODR &= ~GPIO_Pin_8;
			TIM_SetCounter(TIM2,last_time);
			last_time = 20000;
			ServoNum = 0;
			break;

		}

	}
}

// restore position to previous position
void Servo_Restore(int *Pos)
{
	int i;
	for(i=0; i<10; i++)
	{
		ServoPosUs[i] = Pos[i];
	}
}
